#include "commData.h"
#include "protocol.h"
#include "CRC_Generate.h"
#include "protocol_upmotor.h"
#include "GPIO.h"

FunType_bufferTx   up_lowTx;
/*
note: this part is reference from modbus
sci communication parameter:
speed:9600
checksum:even
stopbit:1
slave add:0x01
*/
up_resultInfo	upmotor_result;
up_info 		upmotor_proinfo;

//#define 	upmotor_ID			(1)
//#define   	upmotor_group		(4)
list_prot 	upmotor_queue_tx;
mesg_prot	upmotor_info[1];
uint8_t u8_upmotor_acknum = 0;

void queue_insert_upmotor(pmesg_prot pmesg)
{
	uint8_t pos, status, len, i;
	//uint16_t crc;
	plist_prot pqueue;
	pqueue = &upmotor_queue_tx;
	pos = pqueue->head;
	if(mesg_max==pos)pos = 0;
	status = pmesg->status;
	if(0==status){
		len = pmesg->len;		
		pqueue->mesg_queue[pos].status = status;
		pqueue->mesg_queue[pos].ID = pmesg->ID;
		pqueue->mesg_queue[pos].len = len;
		for(i=0; i<len; i++){
			pqueue->mesg_queue[pos].buffer[i] = pmesg->buffer[i];
		}
		pqueue->head++;
		if(mesg_max==pqueue->head)pqueue->head=0;
		if(pqueue->size<10)pqueue->size++;		
	}
}



uint8_t upmotor_process(pmesg_prot mesg)
{
	//uint8_t count;
	uint8_t offset;
	//uint8_t data_Tx[8];
	//len = mesg->len;
	//buffer index from (2~len+2) is the register result
	offset = 2;
	//pwm
	upmotor_result.pwm = mesg->buffer[offset++];
	upmotor_result.pwm = (upmotor_result.pwm<<8)+mesg->buffer[offset++];
	//current
	upmotor_result.current = mesg->buffer[offset++];
	upmotor_result.current = (upmotor_result.current<<8)+mesg->buffer[offset++];
	//freq
	upmotor_result.freq = mesg->buffer[offset++];
	upmotor_result.freq = (upmotor_result.freq<<8)+mesg->buffer[offset++];
	//position control status
	upmotor_result.pos_cont_status = mesg->buffer[offset++];
	upmotor_result.pos_cont_status = (upmotor_result.pos_cont_status<<8)+mesg->buffer[offset++];
	//position high half
	upmotor_result.pos_high_half = mesg->buffer[offset++];
	upmotor_result.pos_high_half = (upmotor_result.pos_high_half<<8)+mesg->buffer[offset++];
	//position low half
	upmotor_result.pos_low_half = mesg->buffer[offset++];
	upmotor_result.pos_low_half = (upmotor_result.pos_low_half<<8)+mesg->buffer[offset++];
	//reserve time high half
	upmotor_result.res_time_high_half = mesg->buffer[offset++];
	upmotor_result.res_time_high_half = (upmotor_result.res_time_high_half<<8)+mesg->buffer[offset++];
	//reserve time low half
	upmotor_result.res_time_low_half = mesg->buffer[offset++];
	upmotor_result.res_time_low_half = (upmotor_result.res_time_low_half<<8)+mesg->buffer[offset++];
	//in1 volt
	upmotor_result.in1_volt = mesg->buffer[offset++];
	upmotor_result.in1_volt = (upmotor_result.in1_volt<<8)+mesg->buffer[offset++];
	//in2 volt
	upmotor_result.in2_volt = mesg->buffer[offset++];
	upmotor_result.in2_volt = (upmotor_result.in2_volt<<8)+mesg->buffer[offset++];
	//in3 volt
	upmotor_result.in3_volt = mesg->buffer[offset++];
	upmotor_result.in3_volt = (upmotor_result.in3_volt<<8)+mesg->buffer[offset++];
	//div volt
	upmotor_result.div_volt = mesg->buffer[offset++];
	upmotor_result.div_volt = (upmotor_result.div_volt<<8)+mesg->buffer[offset++];
	//sq1 volt
	upmotor_result.sq1_volt = mesg->buffer[offset++];
	upmotor_result.sq1_volt = (upmotor_result.sq1_volt<<8)+mesg->buffer[offset++];
	//sq2 volt
	upmotor_result.sq2_volt = mesg->buffer[offset++];
	upmotor_result.sq2_volt = (upmotor_result.sq2_volt<<8)+mesg->buffer[offset++];
	//in1 input pwm duty
	upmotor_result.in1_input_pwmduty = mesg->buffer[offset++];
	upmotor_result.in1_input_pwmduty = (upmotor_result.in1_input_pwmduty<<8)+mesg->buffer[offset++];
	//in1 input freq
	upmotor_result.in1_input_freq = mesg->buffer[offset++];
	upmotor_result.in1_input_freq = (upmotor_result.in1_input_freq<<8)+mesg->buffer[offset++];
	//in1 input pulse count high half
	upmotor_result.in1_input_pulsecount_high_half = mesg->buffer[offset++];
	upmotor_result.in1_input_pulsecount_high_half = (upmotor_result.in1_input_pulsecount_high_half<<8)+mesg->buffer[offset++];
	//in1 input pulse count low half
	upmotor_result.in1_input_pulsecount_low_half = mesg->buffer[offset++];
	upmotor_result.in1_input_pulsecount_low_half = (upmotor_result.in1_input_pulsecount_low_half<<8)+mesg->buffer[offset++];
	//halt status
	upmotor_result.halt_status = mesg->buffer[offset++];
	upmotor_result.halt_status = (upmotor_result.halt_status<<8)+mesg->buffer[offset++];
	//error status
	upmotor_result.error_status = mesg->buffer[offset++];
	upmotor_result.error_status = (upmotor_result.error_status<<8)+mesg->buffer[offset++];
	//motor speed
	upmotor_result.motor_speed = mesg->buffer[offset++];
	upmotor_result.motor_speed = (upmotor_result.motor_speed<<8)+mesg->buffer[offset++];
	//motor speed ratio 1:10
	upmotor_result.motor_speed_ratio = mesg->buffer[offset++];
	upmotor_result.motor_speed_ratio = (upmotor_result.motor_speed_ratio<<8)+mesg->buffer[offset++];	
	//result process
	u8_upmotor_status = upmotor_result.error_status;
	u8_upmotor_acknum = 0;
	return 0;
}

uint8_t upmotor_rxFunc(void)
{
	return 0;	
}

static uint8_t upmotor_frametx(pmesg_prot mesg)
{
	uint8_t i, index;
	uint8_t buffer[128];
	uint16_t crc = 0;
	//uint32_t id;
	index = 0;
	//ID insert
	for(i=0; i<1; i++){
		buffer[index++] = (uint8_t)((mesg->ID)>>(8*i));
	}
	//buffer insert
	for(i=0; i<mesg->len; i++){
		buffer[index++] = mesg->buffer[i];
	}
	//CRC INSERT
	//crc = GetCrC16(pqueue->mesg_queue[pos].buffer, len);
	crc = CRC16_comm(buffer, index);
	buffer[index++] = (uint8_t)(crc>>0);
	buffer[index++] = (uint8_t)(crc>>8);
	//tx frame
	up_lowTx(buffer, index);
	u8_upmotor_acknum++;
	return 0;
}
uint8_t upmotor_txFunc(void)
{
	uint8_t result;
	uint8_t pos;
	plist_prot pqueue;
	//second, rx data pro
	pqueue = &upmotor_queue_tx;
	//size = pqueue->size;
	//while(pqueue->size>0){
	if(pqueue->size>0){
		pos = pqueue->tail;
		if(pos>=mesg_max)pos=0;
		//result pro
		result = upmotor_frametx(&(pqueue->mesg_queue[pos]));
		pqueue->tail++;
		if(pqueue->tail>=mesg_max)pqueue->tail=0;		
		pqueue->size--;
	}
	return result;
}

uint8_t upmotor_period_callback(void)
{	
	if(upmotor_proinfo.timer<4){
		upmotor_proinfo.timer++;
	}else{
		upmotor_proinfo.timer = 4;
		upmotor_proinfo.step = 0;
		upmotor_proinfo.count = 0;
		upmotor_proinfo.crc16 = 0;
		upmotor_proinfo.crc16_T = 0;
		upmotor_proinfo.len = 0;
	}
	return 0;
}
/*
ask:
01 03 00 20 00 15 85 CF 
resp:
01 03 2A 
00 00 00 00 00 
00 00 01 FF FF 
F8 0D 00 00 00 
00 0C DE 0C DE 
0C E1 00 00 00 
01 00 01 03 E8 
00 00 00 00 00 
00 00 00 00 00 
FF FF 
E7 1C
note: 
    the u8_upmotor_idealtimer should be update by timer
*/

void upmotor_rxFunc_callback(uint8_t data_rx)
{
	static uint16_t crc_t = 0;
	up_Pinfo PproT;
	PproT = &upmotor_proinfo;
	//first, start flag check
	if(PproT->timer>3){		
		upmotor_info[0].status=1;
		upmotor_info[0].ID = data_rx;
		PproT->count = 0;
		if(upmotor_ID==data_rx){
			PproT->step = 1;
			return;
		}				
	}else{
		if(0==PproT->step){
			if(upmotor_ID==data_rx){
				PproT->step = 1;
				PproT->count = 0;
				upmotor_info[0].status=1;
				upmotor_info[0].ID = data_rx;
				upmotor_info[0].buffer[PproT->count++] = data_rx;
				return;
			}
		}
	}
	PproT->timer = 0;
	switch(PproT->step){
		case(1):
			//second, cmd check
			if(0x03==data_rx){
				upmotor_info[0].buffer[PproT->count++] = data_rx;
				PproT->step = 0x02;
			}else{
				//reset
				PproT->step = 0;
				PproT->count = 0;
				upmotor_info[0].status=0;
			}
		break;
		case(2):
			//third, len check
			upmotor_info[0].buffer[PproT->count++] = data_rx;
			PproT->len = upmotor_info[0].len = data_rx;
			PproT->step = 3;
		break;
		case(3):
			//four, data buffer
			if(PproT->count<(PproT->len+3)){
				upmotor_info[0].buffer[PproT->count++] = data_rx;
			}else{
				//five, checksum
				PproT->crc16 = CRC16_comm(upmotor_info[0].buffer, PproT->count);
				upmotor_info[0].buffer[PproT->count++] = data_rx;
				crc_t = data_rx;
				PproT->step = 4;
			}
		break;
		case(4):
			//six, end check
			PproT->crc16_T = data_rx;
			PproT->crc16_T = (PproT->crc16_T<<8) + crc_t;
			upmotor_info[0].buffer[PproT->count++] = data_rx;
			if(PproT->crc16 ==PproT->crc16_T)
			{
				upmotor_info[0].status=0;
				//insert data
				list_insert(&upmotor_info[0],upmotor_group);
			}
			//reset
			PproT->step = 0;
			PproT->count = 0;
		break;
		default:
			PproT->step = 0;
			PproT->count = 0;
		break;
	}
}

/*
Cmds
*/

/*
status check:01 03 00 20 00 15 85 CF
*/
uint8_t upmotor_status_Get(void)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x01;
	count = 0;
	Tmesg.buffer[count++] = 0x03;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x20;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x15;
	//Tmesg.buffer[count++] = 0x85;
	//Tmesg.buffer[count++] = 0xCF;
	Tmesg.len = count;
	queue_insert_upmotor(&Tmesg);
	u8_upmotor_acknum++;
	return 0;
}
/*
setting speed and position:
    01 10 00 44 00 04 08 03 E8 00 01 00 00 00 F4 52 D6 
note:
  the speedHz is 0.1hz为精度
  the positionLen has signed  
*/
uint8_t upmotor_speedandposition_Set(int16_t speedHz, uint16_t positionMode, int32_t positionLen)
{
	uint8_t count;
	mesg_prot Tmesg;
	//uint16_t speedHz = 2000;//0.1hz为精度
	//uint16_t positionMode = 1;//1->相对位置；0->绝对位置
	//uint16_t positionLen = 100;
	//uint16_t crc = 0;
	Tmesg.status = 0;
	Tmesg.ID = 0x01;
	count = 0;
	Tmesg.buffer[count++] = 0x10;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x44;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x04;
	Tmesg.buffer[count++] = 0x08;
	Tmesg.buffer[count++] = (uint8_t)(speedHz>>8);
	Tmesg.buffer[count++] = (uint8_t)(speedHz>>0);
	Tmesg.buffer[count++] = (uint8_t)(positionMode>>8);
	Tmesg.buffer[count++] = (uint8_t)(positionMode>>0);
	Tmesg.buffer[count++] = (uint8_t)(positionLen>>24);
	Tmesg.buffer[count++] = (uint8_t)(positionLen>>16);
	Tmesg.buffer[count++] = (uint8_t)(positionLen>>8);
	Tmesg.buffer[count++] = (uint8_t)(positionLen>>0);
	Tmesg.len = count;
	queue_insert_upmotor(&Tmesg);
	return 0;
}
//01 06 00 40 00 00 88 1E
uint8_t upmotor_stop(void)
{
	uint8_t count;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x01;
	count = 0;
	Tmesg.buffer[count++] = 0x06;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x40;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.buffer[count++] = 0x00;
	Tmesg.len = count;
	queue_insert_upmotor(&Tmesg);
	return 0;
}

/*
user can use this function to change low tx function
*/
void upmotor_lowTxfunc_Register(FunType_bufferTx txFunc)
{
	up_lowTx = txFunc;
}

void upmotor_init(FunType_bufferTx txFunc)
{
	uint8_t i = 0;
	//register function
	func_register_init(upmotor_group, upmotor_rxFunc, upmotor_txFunc, upmotor_process);
	up_lowTx = txFunc;
	for(i=0; i<mesg_max; i++){		
		upmotor_queue_tx.mesg_queue[i].status = 1;
	}
}


uint8_t Ingrap_Action_down(int32_t upmotor_Loopdegree)
{		
	int result = 0;
	if(upmotor_Loopdegree>0)upmotor_Loopdegree = 0-upmotor_Loopdegree;
	upmotor_speedandposition_Set(1000, 1, upmotor_Loopdegree);
	//upmotor_speedandposition_Set(1000, 1, 10);	
	list_check(upmotor_group);
	wait_for_timeout(1000);
	//upmotor_status_Get();
	//wait_for_timeout(100);
	if(upmotor_result.current>700){
		upmotor_speedandposition_Set(0, 1, upmotor_Loopdegree);	
		result = 1;
	}else{
		result = 0;
	}
	return result;
}
uint8_t Ingrap_Action_Up(int32_t upmotor_Loopdegree)
{	
	int result = 0;
    wait_for_timeout(50);
	if(upmotor_Loopdegree<0)upmotor_Loopdegree = 0-upmotor_Loopdegree;
	upmotor_speedandposition_Set(1000, 1, upmotor_Loopdegree);		
	list_check(upmotor_group);
	wait_for_timeout(1000);
	if(upmotor_result.current>700){
		upmotor_speedandposition_Set(0, 1, upmotor_Loopdegree);	
		result = 1;
	}else{
		result = 0;
	}
	return result;
}

uint8_t upmotor_firstPosition_Set(void)
{
	uint8_t result = 0;
	uint8_t firstPos_Volt = 0;
	uint8_t tryCount = 0;
	uint8_t beyondcount = 0;
	uint8_t wait_still = 1;

	firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
	while(1/*!=firstPos_Volt*/){
		upmotor_speedandposition_Set(1000, 1, 50);
		list_check(upmotor_group);
		wait_for_timeout(20);
		//check fisrt position check
		firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
        if(0==firstPos_Volt){
            upmotor_stop();
			list_check(upmotor_group);
			wait_for_timeout(50);
            break;
        }
		//current check
        upmotor_status_Get();
		list_check(upmotor_group);
		wait_for_timeout(20);
		if(upmotor_result.current>700){
			if(0!=firstPos_Volt){
				result = 1;
			}
            upmotor_stop();
			list_check(upmotor_group);
			wait_for_timeout(50);
			break;
		}else{
			tryCount++;
			if(tryCount>20){
				result = 1;
				break;
			}
		}
	}
	wait_for_timeout(50);
    upmotor_stop();
    list_check(upmotor_group);
    wait_for_timeout(50);
	upmotor_speedandposition_Set(1000, 1, -50);	
	list_check(upmotor_group);
	wait_for_timeout(200);
	return result;
}
/*
this function should be call by application periodly
*/
static void upmotor_funcTest(void)
{
	static uint8_t loop = 0;
	loop++;
	//uint16_t speedHz = 2000;//0.1hz为精度
	//uint16_t positionMode = 1;//1->相对位置；0->绝对位置
	//uint16_t positionLen = 100;
	if(loop%2==1){
		upmotor_speedandposition_Set(1000, 1, 100);
	}else{
		upmotor_speedandposition_Set(1000, 1, -100);
	}
}
/*
note : this is only a function test interface, not use for real function
*/
uint8_t upmotor_interfaceTest(void)
{
	uint8_t result = 0;	
	//get the status of upmotor
	upmotor_status_Get();
	upmotor_funcTest();
	//result = list_check(upmotor_group);
	return result;
}
uint8_t upmotor_task(void)
{
	uint8_t result = 0;	
	//get the status of upmotor
	upmotor_status_Get();
	//upmotor_funcTest();
	result = list_check(upmotor_group);
	return result;
}


